#include "supercap.hpp"

#include "can.h"
#include "rtthread.h"

/**
 * @brief 超级电容初始化
 *
 * @return int
 */
int Supercap::start(void) {
    chassis_power_limit_ = 50;
    input_power_limit_   = 40;
    cap_contorl          = 0xffff;
    return 0;
}

void Supercap::init() {
    /* 初始化超级电容定时器 */
    timer_ = rt_timer_create("supercap", timeout, (void *)this, 20, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    /* 启动超级电容定时器 */
    rt_timer_start(timer_);
}

/**
 * @brief 超级电容超时函数
 *
 * @param parameter
 */
void Supercap::timeout(void *parameter) {
    Supercap *t_supercap = (Supercap *)parameter;
    t_supercap->handle();
}

void Supercap::handle() {
    chassis_power_limit_ = game_robot_status.chassis_power_limit;
    input_power_limit_   = uint16_t(game_robot_status.chassis_power_limit * 0.45);
	cap_contorl = 0xffff;
    /* 设置超级电容功率及缓存功率 */
//	  set_buffer_power(0);
//	  set_power(40, 200, 32, 0xffff);
    set_power(chassis_power_limit_*0.8, 200, input_power_limit_, cap_contorl);
//    if (supercap_buffer_flag == true) 
//    {
//        set_buffer_power(0);
//    }
//    else 
//    {
//       set_buffer_power(power_heat_data.chassis_power_buffer);
//    }
}

/**
 * @brief 更新超级电容数据
 *
 * @param data 原始数据
 */
void Supercap::update(uint8_t Data[8]) {
    cap_v  = ((Data[0] << 8) | Data[1]) / 100.0;
    cap_i  = ((Data[2] << 8) | Data[3]) / 100.0;
    state  = (Data[4] << 8) | Data[5];
}

/**
 * @brief 设置目标功率
 *
 * @param chassis_power_limit_, output_power_limmit_, input_power_limit_, cap_contorl
 */
void Supercap::set_power(uint16_t chassis_power_limit_,
                         uint16_t output_power_limmit_,
                         uint16_t input_power_limit_,
                         uint16_t cap_contorl) {
    CAN_TxHeaderTypeDef tx_header;
    uint8_t             tx_data[8];
    uint32_t            tx_mailbox;

    /* 配置消息头 */
    tx_header.StdId              = 0x2FU;
    tx_header.IDE                = CAN_ID_STD;
    tx_header.RTR                = CAN_RTR_DATA;
    tx_header.DLC                = 8;
    tx_header.TransmitGlobalTime = DISABLE;

    /* 计算发送数据 */
    tx_data[0] = chassis_power_limit_ >> 8;
    tx_data[1] = chassis_power_limit_;
    tx_data[2] = output_power_limmit_ >> 8;
    tx_data[3] = output_power_limmit_;
    tx_data[4] = input_power_limit_ >> 8;
    tx_data[5] = input_power_limit_;
    tx_data[6] = cap_contorl >> 8;
    tx_data[7] = cap_contorl;
    HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mailbox);
}

/**
 * @brief 设置缓存功率
 *
 * @param chassis_buffer_power_
 */
void Supercap::set_buffer_power(uint16_t chassis_buff_power_) {
    CAN_TxHeaderTypeDef tx_header;
    uint8_t             tx_data[8];
    uint32_t            tx_mailbox;

    /* 配置消息头 */
    tx_header.StdId              = 0x2EU;
    tx_header.IDE                = CAN_ID_STD;
    tx_header.RTR                = CAN_RTR_DATA;
    tx_header.DLC                = 8;
    tx_header.TransmitGlobalTime = DISABLE;

    /* 计算发送数据 */
    tx_data[0] = chassis_buff_power_ >> 8;
    tx_data[1] = chassis_buff_power_;

    HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mailbox);
}
